/*
 * detection.c
 *
 *  Created on: 24/mag/2011
 *      Author: giosue86
 */
#include "detection.h"

/* Funzioni interne */
void pre_elaboration(IplImage* src, IplImage* dest, CvScalar hsv_min,
		CvScalar hsv_max, CvScalar hsv_min2, CvScalar hsv_max2);
int detecting(IplImage* src, IplImage* th, Balloon balloons[], CvPoint origin);
int distance(int x1, int x2, int y1, int y2);
int isEllipse(CvSeq* contours, IplImage *src);
double angle_detect(CvPoint origine, CvPoint target);
float area_ellipse(float a, float b);

int detection(IplImage* src, IplImage* dest, IplImage* th, Balloon balloons[],
		CvScalar hsv_min, CvScalar hsv_max, CvScalar hsv_min2,
		CvScalar hsv_max2, CvPoint origin) {
	pre_elaboration(src, th, hsv_min, hsv_max, hsv_min2, hsv_max2);
	int n = detecting(dest, th, balloons, origin);
	return n;
}

void pre_elaboration(IplImage* src, IplImage* dest, CvScalar hsv_min,
		CvScalar hsv_max, CvScalar hsv_min2, CvScalar hsv_max2) {

	/* Fase dichiarativa */
	IplImage* hsv = cvCreateImage(cvGetSize(src), IPL_DEPTH_8U, 3);

	IplImage* temp = cvCreateImage(cvGetSize(src), IPL_DEPTH_8U, 1);
	IplImage* temp2 = cvCreateImage(cvGetSize(src), IPL_DEPTH_8U, 1);

	cvCvtColor(src, hsv, CV_BGR2HSV);
	cvInRangeS(hsv, hsv_min, hsv_max, temp);
	cvInRangeS(hsv, hsv_min2, hsv_max2, temp2);
	cvOr(temp, temp2, dest);

	cvErode(dest, dest, NULL, 1);
	cvDilate(dest, dest, NULL, 2);

	cvShowImage("YellowTh", dest);

	cvReleaseImage(&temp2);
	cvReleaseImage(&temp);
	cvReleaseImage(&hsv);

}

int detecting(IplImage* src, IplImage* th, Balloon balloons[], CvPoint origin) {

	CvSeq* contours = 0;
	CvMemStorage* storage = cvCreateMemStorage();
	int n = 0;
	int i = 0;

	i = cvFindContours(th, storage, &contours, sizeof(CvContour),
			CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE);

	if (contours) {

		for (; contours != 0; contours = contours->h_next) {

			if (contours-> total > 6) {
				if (isEllipse(contours, src) == 1) {
					balloons[n].ellipse = cvFitEllipse2(contours);
					if (balloons[n].ellipse.size.height > 25
							&& balloons[n].ellipse.size.width > 15
							&& balloons[n].ellipse.center.y > 144) {

						// Calcolo dell'angolo dell'ellissi n rispetto l'origine
						balloons[n].angle = angle_detect(origin, cvPoint(
								balloons[n].ellipse.center.x,
								balloons[n].ellipse.center.y));

						// Calcolo della distanza dell'ellissi n dal suo centro all'origine
						balloons[n].distance
								= (balloons[n].ellipse.center.x, origin.x, balloons[n].ellipse.center.y, origin.y);

						// Calcolo dell'area dell'ellissi
						balloons[n].area = area_ellipse(
								balloons[n].ellipse.size.height / 2,
								balloons[n].ellipse.size.width / 2);

						n++;
					}
				}
			}
		}
	}
	cvReleaseMemStorage(&storage);
	return n;
}

//int isEllipse(CvSeq* contours, IplImage *src) {
//
//	CvPoint* c;
//	c = (CvPoint*) cvGetSeqElem(contours, 0);
//	int x_max = c->x;
//	int y_xmax = c->y;
//	int x_min = c->x;
//	int y_xmin = c->y;
//	int y_min = c->y;
//	int x_ymin = c->x;
//	int y_max = c->y;
//	int x_ymax = c->x;
//
//	for (int i = 1; i < contours->total; i++) {
//		c = (CvPoint*) cvGetSeqElem(contours, i);
//		if (c->x < x_min) {
//			x_min = c->x;
//			y_xmin = c->y;
//		}
//
//		if (c->x > x_max) {
//			x_max = c->x;
//			y_xmax = c->y;
//		}
//
//		if (c->y < y_min) {
//			y_min = c->y;
//			x_ymin = c->x;
//		}
//		if (c->y > y_max) {
//			y_max = c->y;
//			x_ymax = c->x;
//		}
//	}
//
//	cvLine(src, cvPoint(x_max, y_xmax), cvPoint(x_min, y_xmin), cvScalar(255,
//			255, 255), 1, 8, 0);
//	cvLine(src, cvPoint(x_ymax, y_max), cvPoint(x_ymin, y_min), cvScalar(255,
//			255, 255), 1, 8, 0);
//	if ((y_xmax - y_xmin) < 60 && (y_xmax - y_xmin) > -60 && (x_ymax - x_ymin)
//			< 60 && (x_ymax - x_ymin) > -60) {
//		return 1;
//	} else if (x_min < 10 || x_max > 342 || y_max > 270) {
//		return 1;
//	} else {
//		return 0;
//	}
//}


int isEllipse(CvSeq* contours, IplImage* src) {

	CvMoments moments;
	cvMoments(contours, &moments, 1);

	CvRect rect = cvBoundingRect(contours, 0);
	double a = rect.height;
	double b = rect.width, c;

	double m00, m10, m01, m02, m20, m11;

	m00 = cvGetSpatialMoment(&moments, 0, 0);
	m10 = cvGetSpatialMoment(&moments, 1, 0);
	m01 = cvGetSpatialMoment(&moments, 0, 1);
	m11 = cvGetSpatialMoment(&moments, 1, 1);
	m02 = cvGetSpatialMoment(&moments, 0, 2);
	m20 = cvGetSpatialMoment(&moments, 2, 0);
	//printf("m02=%f m20=%f", m02, m20);
	// TBD check that m00 != 0
	float center_x = m10 / m00;
	float center_y = m01 / m00;

	//printf("area %f \n", m00);

	double mu11, mu10, mu02, mu20;
	mu10 = cvGetCentralMoment(&moments, 1, 0);
	mu11 = cvGetCentralMoment(&moments, 1, 1);
	mu20 = cvGetCentralMoment(&moments, 2, 0);
	mu02 = cvGetCentralMoment(&moments, 0, 2);

	//printf("a= %f b=%f\n", a, b);
	// Calcolo dei coefficienti
	a = (m20 / m00) - pow(center_x, 2);
	b = 2 * (m11 / m00 - (center_x * center_y));
	c = (m02 / m00) - pow(center_y, 2);

	double w, l;
	w = sqrt(6 * (a + c - sqrt(pow(b, 2) + pow(a - c, 2))));
	l = sqrt(6 * (a + c + sqrt(pow(b, 2) + pow(a - c, 2))));

	double angle;
	angle = 0.5 * atan(b / (a - c));
	//printf("w %f l %f angle %f\n", w, l, angle * 57.32);

	if (center_x != 0 && center_y != 0 && m00 > 400 && l > w && w > 20
			&& center_y > src->height / 2 && l > 10) {
		return 1;
	}

	else
		return 0;

}

double angle_detect(CvPoint origine, CvPoint target) {
	double sinAngle;
	sinAngle = (target.x - origine.x) / sqrt((origine.y - target.y)
			* (origine.y - target.y) + (target.x - origine.x) * (target.x
			- origine.x));
	return asin(sinAngle) * 57.2957795131;
}

int distance(int x1, int x2, int y1, int y2) {
	return sqrt((x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1));
}

float area_ellipse(float a, float b) {
	return fabs(a * b * M_PI);
}

bool yellowDetection(IplImage* src,CvScalar hsv_min, CvScalar hsv_max, CvScalar hsv_min2,
		CvScalar hsv_max2, CvPoint origin, int heightROI, int widthROI, int thPercent) {

	IplImage* th = cvCreateImage(cvGetSize(src), IPL_DEPTH_8U, 1);
	pre_elaboration(src, th, hsv_min, hsv_max, hsv_min2, hsv_max2);

	int count = 0;
	int areaROI = heightROI*widthROI;

	for(int i = (origin.y-heightROI); i< origin.y; i++){
		for(int j = (origin.x-widthROI/2); j< (origin.x+widthROI/2); j++){
			CvScalar point = cvGet2D(th,i,j);
			if(point.val[0]>250)
				count++;
			cvRectangle(src,cvPoint(origin.x-(widthROI/2),origin.y-heightROI),cvPoint(origin.x+(widthROI/2),origin.y),cvScalar(255,255,255),1);
		}
	}
	cvReleaseImage(&th);


	//printf("\nPixel Gialli Rilevati%d in %d Pixel\n",count,areaROI);

	if(count>=(areaROI*thPercent/100))
		return true;
	else
		return false;
}
